
//   utilities.cpp,  6.2.07, pm-n.


#include "Utilities.h"
#include <GL/glut.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

float  gaussPeak( float my_x, float my_y,
                  float sigma_x, float sigma_y,
                  float x, float y )
{
   float xn, yn;
   xn = (x-my_x)/sigma_x;
   yn = (y-my_y)/sigma_y;
   return ( exp( -(xn*xn + yn*yn ) ) );
}


 void  errorAndExit( char *text )
 {
   printf( "\nError: %s\n", text );
   exit(1);
 }

float euclidianDistance(float* p1, float* p2){
  return sqrt((p1[0]-p2[0])*(p1[0]-p2[0])
	      +(p1[1]-p2[1])*(p1[1]-p2[1])
	      +(p1[2]-p2[2])*(p1[2]-p2[2])
	      );
}


void normalize(float *changeVector, float *normalized, float strength) {
  float x = 0;
  float y = 0;
  float z = 0;

  x = changeVector[0];
  y = changeVector[1];
  z = changeVector[2];

  float length = sqrt(x*x + y*y + z*z) / strength;

  if(length!=0) {
    normalized[0] = x / length;
    normalized[1] = y / length;
    normalized[2] = z / length;
  }
}


void vectorAddF(float * r, float * v1, float * v2) 
{
  r[0] = v1[0] + v2[0];
  r[1] = v1[1] + v2[1];
  r[2] = v1[2] + v2[2];
}

void vectorSubF(float * r, float * v1, float * v2) 
{
  r[0] = v1[0] - v2[0];
  r[1] = v1[1] - v2[1];
  r[2] = v1[2] - v2[2];
}

void addToVectorF(float * r, float * v1) 
{
  r[0] += v1[0];
  r[1] += v1[1];
  r[2] += v1[2];
}

void subFromVectorF(float * r, float * v1) 
{
  r[0] -= v1[0];
  r[1] -= v1[1];
  r[2] -= v1[2];
}

void scaleVectorF(float * v, float scale) 
{
  v[0] = v[0]*scale;
  v[1] = v[1]*scale;
  v[2] = v[2]*scale;
}


float getRand() 
{
  return (((float)rand())/RAND_MAX);
}


void cutVector(float * v, float offset) 
{
  if((v[0]>0&&v[0]<offset)||
     (v[0]<0&&v[0]>-offset)) {
    v[0] = 0;
  }
  if((v[1]>0&&v[1]<offset)||
     (v[1]<0&&v[1]>-offset)) {
    v[1] = 0;
  }
  if((v[2]>0&&v[2]<offset)||
     (v[2]<0&&v[2]>-offset)) {
    v[2] = 0;
  }
}

float vectorLength(float * v) 
{
  return sqrt(v[0]*v[0]+v[1]*v[1]*v[2]*v[2]);
}

// This function prints a column oriented matrix to standart out.
void print4x4(const float * m){

  for(int i = 0; i < 4; i++){
    printf("| %10f & %10f | %10f | %10f |\n",
	   m[4*0+i],m[4*1+i],m[4*2+i],m[4*3+i]);
   }
}

void reportGLError(char * message, char * filename, long line){
	int errorCode;
	if((errorCode = glGetError())!= GL_NO_ERROR){
		printf("GL Error %s in file %s, line %d, %s\n", gluErrorString(errorCode), filename, line, message);
	}
}